/*
 * 版权声明：本软件采用双许可证授权（GPLv3开源许可证 + 商业许可证）
 * 具体许可证信息请参考随软件分发的 LICENSE.md 文件
 */

// 引入标准库：输入输出（stdio）、内存分配（stdlib）、字符串处理（string）、64位整数格式化（inttypes.h）
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <inttypes.h>

// 引入SOEM库头文件：SOEM（Simple Open EtherCAT Master）是开源EtherCAT主站协议栈核心库
#include "soem/soem.h"

// 宏定义：EtherCAT从站监控超时时间（单位：ms），超过此时间判定从站通信异常
#define EC_TIMEOUTMON 500

// 宏定义：1秒对应的纳秒数（1s = 10^9 ns），用于时间单位转换
#define NSEC_PER_SEC  1000000000

// 全局变量：IO映射缓存区（大小4096字节），用于存储主站与从站的过程数据（输入/输出）
static uint8 IOmap[4096];
// 全局变量：线程句柄（threadrt：EtherCAT实时周期线程；thread1：从站错误处理线程）
static OSAL_THREAD_HANDLE threadrt, thread1;
// 全局变量：期望工作计数器（WKC），用于判断主从通信是否正常（WKC=成功接收的从站反馈数）
static int expectedWKC;
// 全局变量：实际工作计数器（记录每次主站接收从站反馈的数量）
static int wkc;
// 全局变量：状态标志位
// mappingdone：IO映射完成标志（1=完成，0=未完成）；dorun：实时线程运行使能（1=运行，0=暂停）
// inOP：EtherCAT网络是否进入OP（Operational）状态；dowkccheck：WKC异常计数（超过阈值触发错误处理）
static int mappingdone, dorun, inOP, dowkccheck;
// 全局变量：当前EtherCAT从站组编号（SOEM支持多组从站，此处默认单组0）
static int currentgroup = 0;
// 全局变量：EtherCAT周期计数（记录实时线程运行的周期次数）
static int cycle = 0;
// 全局变量：EtherCAT周期时间（默认1000000ns=1ms，可通过命令行参数修改）
static int64_t cycletime = 1000000;

// 全局变量：EtherCAT上下文结构体（存储SOEM协议栈的核心配置：从站列表、DC时钟、组配置等）
static ecx_contextt ctx;

/* 函数功能：给ec_timet类型的时间（SOEM定义的时间结构体）增加指定纳秒数 */
void add_time_ns(ec_timet *ts, int64 addtime)
{
   // 定义临时ec_timet结构体，用于存储待添加的时间（拆分秒和纳秒）
   ec_timet addts;

   // 计算待添加时间的纳秒部分：addtime对1秒（NSEC_PER_SEC）取余，得到不足1秒的纳秒数
   addts.tv_nsec = addtime % NSEC_PER_SEC;
   // 计算待添加时间的秒部分：减去纳秒部分后除以1秒，得到完整的秒数
   addts.tv_sec = (addtime - addts.tv_nsec) / NSEC_PER_SEC;
   // 调用SOEM的时间合并函数：将ts（原时间）与addts（待加时间）合并，结果存回ts
   osal_timespecadd(ts, &addts, ts);
}

// 全局变量：DC（分布式时钟）同步的PI控制器参数
static float pgain = 0.01f;    // 比例增益（调整时间偏差的响应速度）
static float igain = 0.00002f; // 积分增益（消除长期时间偏差累积）
// 全局变量：Linux主站与从站DC时钟的同步偏移（示例设为500us，即主站比从站DC晚500us同步）
static int64 syncoffset = 500000;
// 全局变量：主站与从站DC时钟的时间误差（单位：ns）
static int64 timeerror;

/* 函数功能：PI控制器计算，使Linux主站时间与从站DC时钟同步 */
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
{
   // 静态变量：积分项（仅初始化一次，累积历史时间误差，消除静差）
   static int64 integral = 0;
   // 局部变量：计算的时间偏差（主从时间差）
   int64 delta;

   // 1. 计算主从时间偏差delta：
   // reftime=从站DC时间；syncoffset=主从同步偏移；cycletime=周期时间
   // 对周期取余是为了处理“时间环绕”（如DC时间超过周期后的偏差计算）
   delta = (reftime - syncoffset) % cycletime;
   // 2. 处理偏差符号：若delta>周期的1/2，说明实际偏差为负（需调整为负偏差，避免绕远路同步）
   if (delta > (cycletime / 2))
   {
      delta = delta - cycletime;
   }
   // 3. 计算最终时间误差：取delta的负值（使主站时间向从站DC时间对齐）
   timeerror = -delta;
   // 4. 累积积分项：将当前时间误差加入积分（消除长期静差）
   integral += timeerror;
   // 5. PI控制器输出：比例项（时间误差×比例增益）+ 积分项（积分×积分增益），结果存到offsettime（主站需补偿的时间）
   *offsettime = (int64)((timeerror * pgain) + (integral * igain));
}

/* 函数功能：EtherCAT实时周期线程（核心线程，负责主从过程数据收发与DC同步）
 * OSAL_THREAD_FUNC_RT：SOEM定义的实时线程函数宏（确保线程按实时优先级调度）
 */
OSAL_THREAD_FUNC_RT ecatthread(void)
{
   // 局部变量：ec_timet类型时间，用于记录周期起始时间
   ec_timet ts;
   // 局部变量：临时变量，用于将时间对齐到毫秒
   int ht;
   // 静态变量：时间补偿偏移（由ec_sync函数计算，用于主站时间对齐从站DC）
   static int64_t toff = 0;

   // 初始化实时线程运行使能为0（暂停状态，等待IO映射完成）
   dorun = 0;
   // 循环等待IO映射完成（mappingdone=1时退出等待），避免线程提前运行导致数据错误
   while (!mappingdone)
   {
      // 等待期间每100us休眠一次，降低CPU占用
      osal_usleep(100);
   }

   // 1. 初始化周期起始时间：获取Linux系统的单调时钟（不受系统时间修改影响，适合实时场景）
   osal_get_monotonic_time(&ts);
   // 将时间对齐到最近的毫秒（避免初始时间有微小偏差）：
   // ts.tv_nsec/1000000=当前毫秒数，+1后×1000000=对齐到下一个毫秒的起始
   ht = (ts.tv_nsec / 1000000) + 1; 
   ts.tv_nsec = ht * 1000000;

   // 2. 发送初始过程数据：主站向从站发送第一次过程数据（初始化从站输出）
   ecx_send_processdata(&ctx);

   // 3. 实时周期循环（线程核心逻辑，永久运行）
   while (1)
   {
      // 计算下一个周期的起始时间：当前时间（ts） + 周期时间（cycletime） + 时间补偿（toff）
      add_time_ns(&ts, cycletime + toff);
      // 休眠到下一个周期起始时间：用单调时钟休眠，确保周期精度（实时线程关键）
      osal_monotonic_sleep(&ts);

      // 若线程运行使能（dorun=1），执行过程数据收发与同步逻辑
      if (dorun > 0)
      {
         // 周期计数+1（记录当前是第几个运行周期）
         cycle++;

         // 接收从站过程数据：ctx=EtherCAT上下文；EC_TIMEOUTRET=接收超时时间（SOEM默认）
         // 返回值wkc=实际接收的从站反馈数，用于判断通信是否正常
         wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET);

         // 检查WKC是否与期望一致：不一致则WKC异常计数+1，否则重置为0
         if (wkc != expectedWKC)
            dowkccheck++;
         else
            dowkccheck = 0;

         // 若从站支持DC时钟（ctx.slavelist[0].hasdc=1）且通信正常（wkc>0），执行DC同步
         if (ctx.slavelist[0].hasdc && (wkc > 0))
         {
            // 调用PI同步函数：输入从站DC时间（ctx.DCtime）、周期，输出时间补偿（toff）
            ec_sync(ctx.DCtime, cycletime, &toff);
         }

         // 处理从站邮箱通信（CoE协议：CANopen over EtherCAT）：
         // 0=不等待；4=处理4个邮箱帧（SOEM推荐值，确保及时处理从站CoE请求）
         ecx_mbxhandler(&ctx, 0, 4);

         // 向从站发送当前周期的过程数据（主站→从站，更新从站输出）
         ecx_send_processdata(&ctx);
      }
   }
}

/* 函数功能：从站错误处理线程（非实时，负责监测从站状态异常并尝试恢复）
 * OSAL_THREAD_FUNC：SOEM定义的普通线程函数宏（非实时优先级）
 */
OSAL_THREAD_FUNC ecatcheck(void)
{
   // 局部变量：从站索引（遍历所有从站时使用）
   int slaveix;

   // 永久循环：持续监测从站状态
   while (1)
   {
      // 若网络已进入OP状态，且满足以下任一条件，触发错误处理：
      // 1. WKC异常计数>2（连续3次通信异常）；2. 从站组标记需要检查状态（docheckstate=1）
      if (inOP && ((dowkccheck > 2) || ctx.grouplist[currentgroup].docheckstate))
      {
         // 重置状态检查标记（避免重复触发错误处理）
         ctx.grouplist[currentgroup].docheckstate = FALSE;

         // 读取所有从站的当前状态（获取从站的AL状态、错误码等）
         ecx_readstate(&ctx);

         // 遍历所有从站（从站索引从1开始，0为虚拟主站）
         for (slaveix = 1; slaveix <= ctx.slavecount; slaveix++)
         {
            // 获取当前从站的配置结构体（存储该从站的状态、IO地址、DC信息等）
            ec_slavet *slave = &ctx.slavelist[slaveix];

            // 若当前从站属于当前组，且状态不是OP（Operational），处理该从站异常
            if ((slave->group == currentgroup) && (slave->state != EC_STATE_OPERATIONAL))
            {
               // 标记需要持续检查状态（直到从站恢复OP）
               ctx.grouplist[currentgroup].docheckstate = TRUE;

               // 情况1：从站状态为“SAFE_OP + ERROR”（安全操作+错误，常见于通信中断后恢复）
               if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
               {
                  // 打印错误信息：提示从站编号及状态
                  printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slaveix);
                  // 设置从站状态为“SAFE_OP + ACK”（确认错误，触发从站清除错误）
                  slave->state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
                  // 向从站写入新状态（执行错误确认）
                  ecx_writestate(&ctx, slaveix);
               }
               // 情况2：从站状态为“SAFE_OP”（安全操作，未进入OP）
               else if (slave->state == EC_STATE_SAFE_OP)
               {
                  // 打印警告信息：提示从站需切换到OP
                  printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slaveix);
                  // 设置从站目标状态为OP
                  slave->state = EC_STATE_OPERATIONAL;
                  // 若从站邮箱处理状态为“丢失”（ECT_MBXH_LOST），重置为“周期处理”（ECT_MBXH_CYCLIC）
                  if (slave->mbxhandlerstate == ECT_MBXH_LOST) 
                     slave->mbxhandlerstate = ECT_MBXH_CYCLIC;
                  // 向从站写入OP状态（触发从站进入运行状态）
                  ecx_writestate(&ctx, slaveix);
               }
               // 情况3：从站状态为“非初始/非丢失”（如PRE_OP，可重新配置）
               else if (slave->state > EC_STATE_NONE)
               {
                  // 重新配置从站：输入上下文、从站索引、超时时间，返回配置后的状态
                  if (ecx_reconfig_slave(&ctx, slaveix, EC_TIMEOUTMON) >= EC_STATE_PRE_OP)
                  {
                     // 标记从站未丢失
                     slave->islost = FALSE;
                     // 打印信息：从站重新配置成功
                     printf("MESSAGE : slave %d reconfigured\n", slaveix);
                  }
               }
               // 情况4：从站未标记为“丢失”，但状态异常（需二次确认）
               else if (!slave->islost)
               {
                  // 二次检查从站状态：等待从站进入OP，超时时间为EC_TIMEOUTRET
                  ecx_statecheck(&ctx, slaveix, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
                  // 若二次检查后状态仍为“无状态”（EC_STATE_NONE），判定从站丢失
                  if (slave->state == EC_STATE_NONE)
                  {
                     // 标记从站丢失
                     slave->islost = TRUE;
                     // 标记从站邮箱处理状态为“丢失”
                     slave->mbxhandlerstate = ECT_MBXH_LOST;
                     // 若从站有输入数据缓存（Ibytes>0），将输入数据清零（避免脏数据）
                     if (slave->Ibytes)
                     {
                        memset(slave->inputs, 0x00, slave->Ibytes);
                     }
                     // 打印错误信息：从站丢失
                     printf("ERROR : slave %d lost\n", slaveix);
                  }
               }
            }

            // 若从站已标记为“丢失”，尝试恢复
            if (slave->islost)
            {
               // 情况1：从站状态≤初始状态（INIT），需执行恢复流程
               if (slave->state <= EC_STATE_INIT)
               {
                  // 恢复从站：输入上下文、从站索引、超时时间，成功则返回1
                  if (ecx_recover_slave(&ctx, slaveix, EC_TIMEOUTMON))
                  {
                     // 标记从站未丢失
                     slave->islost = FALSE;
                     // 打印信息：从站恢复成功
                     printf("MESSAGE : slave %d recovered\n", slaveix);
                  }
               }
               // 情况2：从站状态>初始状态（已重新上线，仅需清除丢失标记）
               else
               {
                  // 标记从站未丢失
                  slave->islost = FALSE;
                  // 打印信息：从站重新找到
                  printf("MESSAGE : slave %d found\n", slaveix);
               }
            }
         }

         // 若所有从站恢复正常（无需持续检查状态），打印正常信息
         if (!ctx.grouplist[currentgroup].docheckstate)
            printf("OK : all slaves resumed OPERATIONAL.\n");
         // 重置WKC异常计数（错误处理完成）
         dowkccheck = 0;
      }

      // 非实时线程每10ms休眠一次，降低CPU占用（避免干扰实时线程）
      osal_usleep(10000);
   }
}

/* 函数功能：EtherCAT网络启动流程（初始化→配置→切换OP状态→数据收发→停机） */
void ecatbringup(char *ifname)
{
   // 打印启动信息：EtherCAT开始启动
   printf("EtherCAT Startup\n");

   // 1. 初始化EtherCAT主站：输入上下文、网卡名（如enp2s0），返回1=成功，0=失败
   int rv = ecx_init(&ctx, ifname);
   if (rv) // 若初始化成功，执行后续配置
   {
      // 2. 初始化从站配置：扫描总线上的从站，自动获取从站信息（数量、型号、IO大小等）
      ecx_config_init(&ctx);

      // 若扫描到从站（slavecount>0），执行IO映射与状态切换
      if (ctx.slavecount > 0)
      {
         // 获取当前从站组（组0）的配置结构体
         ec_groupt *group = &ctx.grouplist[0];

         // 3. 映射IO：将从站的输入/输出数据映射到IOmap缓存区，0=映射组0
         ecx_config_map_group(&ctx, IOmap, 0);

         // 4. 计算期望工作计数器（expectedWKC）：
         // 公式=输出WKC×2（主站发送→从站确认） + 输入WKC（从站发送→主站确认）
         expectedWKC = (group->outputsWKC * 2) + group->inputsWKC;

         // 打印从站配置信息：找到的从站数量
         printf("%d slaves found and configured.\n", ctx.slavecount);

         // 打印IO段信息：组0的IO段数量+各段地址（SOEM内部IO分段管理，用于高效数据收发）
         printf("segments : %d : %d %d %d %d\n",
                group->nsegments,
                group->IOsegment[0],
                group->IOsegment[1],
                group->IOsegment[2],
                group->IOsegment[3]);

         // 5. 标记IO映射完成（触发实时线程退出等待，开始运行）
         mappingdone = 1;

         // 6. 配置分布式时钟（DC）：同步所有从站的时钟，确保主从时间一致
         ecx_configdc(&ctx);

         // 7. 将支持CoE协议的从站添加到周期邮箱处理（CoE用于配置从站参数，如PDO/SDO）
         int sdoslave = -1; // 记录支持SDO的从站编号（用于后续SDO读取示例）
         for (int si = 1; si <= ctx.slavecount; si++)
         {
            // 获取当前从站结构体
            ec_slavet *slave = &ctx.slavelist[si];
            // 若从站CoE详情>0（支持CoE协议）
            if (slave->CoEdetails > 0)
            {
               // 将从站添加到周期邮箱处理队列（自动处理CoE请求）
               ecx_slavembxcyclic(&ctx, si);
               // 记录第一个支持SDO的从站编号
               sdoslave = si;
               // 打印信息：从站已添加到邮箱处理
               printf(" Slave %d added to cyclic mailbox handler\n", si);
            }
         }

         // 8. 等待主从时钟同步：休眠1秒（确保DC时钟同步稳定）
         dorun = 1; // 使能实时线程运行
         osal_usleep(1000000);

         // 9. 切换网络到OP状态：
         // 设置虚拟主站（slavelist[0]）的目标状态为OP（所有从站会跟随切换）
         ctx.slavelist[0].state = EC_STATE_OPERATIONAL;
         // 向所有从站写入OP状态指令
         ecx_writestate(&ctx, 0);
         // 等待网络进入OP状态：超时时间为EC_TIMEOUTSTATE（SOEM默认）
         ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);

         // 检查网络是否成功进入OP状态
         if (ctx.slavelist[0].state != EC_STATE_OPERATIONAL)
         {
            // 若未进入OP，读取所有从站状态，打印异常从站信息
            ecx_readstate(&ctx);
            for (int si = 1; si <= ctx.slavecount; si++)
            {
               ec_slavet *slave = &ctx.slavelist[si];
               // 打印状态异常的从站：编号、状态码、错误码、错误描述
               if (slave->state != EC_STATE_OPERATIONAL)
               {
                  printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                         si,
                         slave->state,
                         slave->ALstatuscode,
                         ec_ALstatuscode2string(slave->ALstatuscode)); // 错误码转字符串描述
               }
            }
         }
         else // 若成功进入OP状态，执行过程数据收发与打印
         {
            int size; // 局部变量：IO数据长度（用于限制打印字节数）

            // 打印信息：EtherCAT已进入OP状态
            printf("EtherCAT OP\n");
            // 标记网络进入OP状态（触发错误处理线程开始监测）
            inOP = TRUE;

            // 10. 周期性打印过程数据：循环5000次，每次休眠20ms（总时长100秒）
            for (int i = 0; i < 5000; i++)
            {
               // 打印核心信息：周期数、WKC、从站DC时间、主从时间误差（dt=timeerror）
               printf("Processdata cycle %5d , Wck %3d, DCtime %12" PRId64 ", dt %8" PRId64 ", O:",
                      cycle,
                      wkc,
                      ctx.DCtime,
                      timeerror); // PRId64：跨平台打印int64_t的格式符

               // 打印主站→从站的输出数据（O：Output）：最多打印8字节（或实际输出长度，取较小值）
               size = group->Obytes < 8 ? group->Obytes : 8;
               for (int j = 0; j < size; j++)
               {
                  // 从虚拟主站的输出缓存（slavelist[0].outputs）读取数据，按16进制打印
                  printf(" %2.2x", *(ctx.slavelist[0].outputs + j));
               }

               // 打印从站→主站的输入数据（I：Input）：最多打印8字节（或实际输入长度，取较小值）
               printf(" I:");
               size = group->Ibytes < 8 ? group->Ibytes : 8;
               for (int j = 0; j < size; j++)
               {
                  // 从虚拟主站的输入缓存（slavelist[0].inputs）读取数据，按16进制打印
                  printf(" %2.2x", *(ctx.slavelist[0].inputs + j));
               }

               // 回车符（\r）：使后续打印覆盖当前行，避免日志刷屏
               printf("\r");
               // 强制刷新标准输出（避免缓冲区导致日志延迟显示）
               fflush(stdout);

               // 11. 示例：从支持SDO的从站读取参数（SDO：服务数据对象，用于非周期配置）
               uint32_t value = 0; // 存储SDO读取结果
               int size = sizeof(value); // 读取数据长度（4字节，uint32_t）
               if (sdoslave > 0) // 若存在支持SDO的从站
               {
                  value = 0; // 初始化读取结果
                  // SDO读取：输入上下文、从站编号、索引（0x1018=从站身份信息）、子索引（0x02=供应商ID）
                  // FALSE=非阻塞读取；EC_TIMEOUTRXM=SDO读取超时时间；返回值sdo_wkc=SDO通信结果
                  int sdo_wkc = ecx_SDOread(&ctx, sdoslave, 0x1018, 0x02, FALSE, &size, &value, EC_TIMEOUTRXM);
                  (void)sdo_wkc; // 强制转换为void，避免“未使用变量”的编译警告
               }

               // 每次循环休眠20ms（控制日志打印频率，避免占用过多CPU）
               osal_usleep(20000);
            }

            // 12. 打印循环结束：换行（避免后续信息与当前行重叠）
            printf("\n");
            // 禁用实时线程运行
            dorun = 0;
            // 标记网络退出OP状态
            inOP = FALSE;
         }

         // 13. 网络停机流程1：切换到SAFE_OP状态（安全操作状态，从站停止输出）
         printf("EtherCAT to SAFE_OP\n");
         ctx.slavelist[0].state = EC_STATE_SAFE_OP;
         ecx_writestate(&ctx, 0);
         // 等待网络进入SAFE_OP状态
         ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);

         // 14. 网络停机流程2：切换到INIT状态（初始状态，从站复位配置）
         printf("EtherCAT to INIT\n");
         ctx.slavelist[0].state = EC_STATE_INIT;
         ecx_writestate(&ctx, 0);
         // 等待网络进入INIT状态
         ecx_statecheck(&ctx, 0, EC_STATE_INIT, EC_TIMEOUTSTATE);
      }
   }
}

/* 函数功能：程序入口（解析命令行参数→创建线程→启动EtherCAT网络） */
int main(int argc, char *argv[])
{
   // 打印程序信息：SOEM版本+程序名
   printf("SOEM (Simple Open EtherCAT Master)\nec_sample\n");

   // 解析命令行参数2：若传入第3个参数（argv[2]），修改EtherCAT周期时间（单位：us→转ns）
   if (argc > 2)
      cycletime = atoi(argv[2]) * 1000; // atoi：字符串转整数；×1000=us→ns

   // 解析命令行参数1：若传入第2个参数（argv[1]，网卡名），启动EtherCAT流程
   if (argc > 1)
   {
      // 1. 创建EtherCAT实时周期线程：
      // threadrt=线程句柄；128000=线程栈大小（128KB）；ecatthread=线程函数；NULL=无参数
      osal_thread_create_rt(&threadrt, 128000, &ecatthread, NULL);

      // 2. 创建从站错误处理线程：
      // thread1=线程句柄；128000=线程栈大小；ecatcheck=线程函数；NULL=无参数
      osal_thread_create(&thread1, 128000, &ecatcheck, NULL);

      // 3. 启动EtherCAT网络：传入网卡名（argv[1]，如enp2s0）
      ecatbringup(argv[1]);
   }
   else // 若未传入参数，打印用法说明+枚举可用网卡
   {
      // 局部变量：网卡适配器结构体（存储网卡名、描述等）
      ec_adaptert *adapter = NULL;
      ec_adaptert *head = NULL;

      // 打印用法：程序名+网卡名+[周期（us）]
      printf("Usage: ec_sample ifname1 [cycletime]\n");
      printf("ifname = eth0 for example\n");
      printf("cycletime in us\n");

      // 枚举并打印所有可用的网卡
      printf("\nAvailable adapters:\n");
      // 调用SOEM函数获取网卡列表，返回链表头
      head = adapter = ec_find_adapters();
      // 遍历网卡链表，打印每个网卡的名称和描述
      while (adapter != NULL)
      {
         printf("    - %s  (%s)\n", adapter->name, adapter->desc);
         adapter = adapter->next; // 指向链表下一个网卡
      }
      // 释放网卡列表内存（避免内存泄漏）
      ec_free_adapters(head);
   }

   // 打印程序结束信息
   printf("End program\n");
   // 主函数返回0（程序正常退出）
   return (0);
}
